% A is skbeta_triang
syms A0 A1 A2 A3 A4 A5 A6 A7 A8 real
syms A9 A10 A11 A12 A13 A14 A15 A16 A17 real
syms A18 A19 A20 A21 A22 A23 A24 A25 A26 real
syms A27 A28 A29 A30 A31 A32 A33 A34 A35 real
syms r0 r1 r2 r3 real
syms g0 g1 g2 real
syms w0 w1 w2 real
syms m mass real
syms beta0 beta1 beta2 real
syms p0 p1 p2 p3 p4 p5 p6 p7 p8 real
syms t0 t1 t2 t3 t4 t5 t6 t7 t8 real
syms z0 z1 z2 z3 z4 z5 z6 z7 z8 real
syms f0 f1 f2 f3 f4 f5 real;
syms b0 b1 b2 real;
syms phi0 phi1 phi2 phi3 phi4 phi5 phi6 phi7 phi8 real
syms psi0 psi1 psi2 psi3 psi4 psi5 psi6 psi7 psi8 real

syms m0 m1 m2 real;
syms lnull real;

M = [m0 m1 m2];

PHI = [phi0 phi1 phi2;
       phi3 phi4 phi5;
       phi6 phi7 phi8];
   
PSI = [psi0 psi1 psi2;
       psi3 psi4 psi5;
       psi6 psi7 psi8];

D = [0 0;
     1 0;
     0 1];
 H = [1 0 0]';
 
 tmp = D'*(PHI+PSI)*D;
 
 T = [t4 t5;t6 t7];
 W = [w0;w1;w2];
 X = D*T*D';
 Wa = X*PHI;
   
PHI13 = [phi0;
         phi1;
         phi2];


B = [b0;b1;b2];
F = [f0;f1;f2];
F6 = [f0;f1;f2;f3;f4;f5];

T = [t0 t1 t2;
     t3 t4 t5;
     t6 t7 t8];
 
 Z = [z0 z1 z2;
      z3 z4 z5;
      z6 z7 z8];
  
 PHI = [p0 p1 p2;
        p3 p4 p5;
        p6 p7 p8];

% I is Ik3
syms I0 I1 I2 I3 I4 I5 I6 I7 I8 real
syms I9 I10 I11 I12 I13 I14 I15 I16 I17 real
syms I18 I19 I20 I21 I22 I23 I24 I25 I26 real
syms I27 I28 I29 I30 I31 I32 I33 I34 I35 real
w = [w0 ;w1 ;w2];
r = [r0 ;r1 ;r2];
g = [g0; g1 ; g2];
r2D = [r0;r1];
g2D = [g0;g1];

A = [   A0  A1  A2  A3  A4  A5;
        A6  A7  A8  A9  A10 A11;
        A12 A13 A14 A15 A16 A17;
        A18 A19 A20 A21 A22 A23;
        A24 A25 A26 A27 A28 A29;
        A30 A31 A32 A33 A34 A35];
    
lA = [  A0  A1  A2  0  0  0;
        A6  A7  A8  0  0  0;
        A12 A13 A14 0  0  0;
        A18 A19 A20 1  0  0;
        A24 A25 A26 0  1  0;
        A30 A31 A32 0  0  1];

I = [   I0  I1  I2  I3  I4  I5;
        I6  I7  I8  I9  I10 I11;
        I12 I13 I14 I15 I16 I17;
        I18 I19 I20 I21 I22 I23;
        I24 I25 I26 I27 I28 I29;
        I30 I31 I32 I33 I34 I35];  
    
syms Q0 Q1 Q2 Q3 Q4 Q5 Q6 Q7 Q8 real
syms Q9 Q10 Q11 Q12 Q13 Q14 Q15 Q16 Q17 real
syms Q18 Q19 Q20 Q21 Q22 Q23 Q24 Q25 Q26 real
syms Q27 Q28 Q29 Q30 Q31 Q32 Q33 Q34 Q35 real

Q = [   Q0  Q1  Q2  Q3  Q4  Q5;
        Q6  Q7  Q8  Q9  Q10 Q11;
        Q12 Q13 Q14 Q15 Q16 Q17;
        Q18 Q19 Q20 Q21 Q22 Q23;
        Q24 Q25 Q26 Q27 Q28 Q29;
        Q30 Q31 Q32 Q33 Q34 Q35];  
    
In = [I0  I1  I2;
      I3  I4  I5;
      I6  I7  I8];
Is = [I0 -r2 r1;
      r2 m 0;
      -r1 0 m];
  
Rot2d = [r0 r1;
         r2 r3];
Rot3d = [p0  p1  p2;
         p3  p4  p5;
        p6  p7  p8];
Rot3D = [p0  p1  0;
         p2  p3  0;
        0   0 1 ];
    
spat_rot3d = [Rot3d zeros(3);
            zeros(3) Rot3d];

spat_rot3D = [Rot3D zeros(3);
            zeros(3) Rot3D];
pk_C_k = [1 0 0;
          0 r0 r1;
          0 r2 r3];
  
betaX = [0      -beta2  beta1;
         beta2  0       -beta0;
        -beta1  beta0   0];

skbeta = [eye(3,3) betaX;
          zeros(3,3) eye(3,3)];
iskbeta = inv(skbeta);
tskbeta = skbeta';
itskbeta = inv(tskbeta);


rx = [0 -r2 r1;
      r2 0 -r0;
      -r1 r0 0];
wx = [0 -w2 w1;
      w2 0 -w0;
      -w1 w0 0];
  
Sf = [eye(3,3) rx;
      zeros(3,3) eye(3,3)];
tSf = Sf';
iSf = inv(Sf);
itSf = inv(tSf);
%     
D = [0 0 1 0 0 0 ;
     0 0 0 1 0 0 ;
     0 0 0 0 1 0]';
 
Pkk = [0 0 1 0 0 0]';
Dkk = [1 0 0 0 0 0;
       0 1 0 0 0 0;
       0 0 0 1 0 0;
       0 0 0 0 1 0;
       0 0 0 0 0 1]';
pkk = [1 0 0]';
 
J = (In - m*rx*rx);
I_d = [ J    m*rx;
       -m*rx m*eye(3)];
Ikk = [Z m*rx;
      -m*rx m*eye(3)];
Fk = [-wx*J*w;
    -m*cross(w,cross(w,r))];

lLAMBDA = (eye(6)-eye(6)*Pkk*Pkk')*lnull;
LAMBDA = iSf*(lLAMBDA+Ikk)*itSf;

syms inertia real
syms s0 s1 real

SfH1 = [1 r1 -r0;
        0 1 0;
        0 0 1];
SfH2 = [1 -r1 r0;
        0 1 0;
        0 0 1];
%     
% sM = [inertia 0      0;
%       0       mass   0;
%       0       0      mass];


HH =  spat_rot3d*Pkk;


% sbeta = D'*skbeta*D
L = eye(6)-I*HH*HH'/m;
P = iskbeta*(lA*Q + Ikk)*itskbeta;
% ccode(P,'file','test');
keyboard
[T1 T2 T3] = textread('test','%s%c%s');

CHARS{1} = 'I';
CHARS{2} = 'Q';
CHARS{3} = 't';
CHARS{4} = 'A';
CHARS{5} = 'r';

% CHARS{4} = 't';
% CHARS{5} = 'I';
% 
% 
[rowt,colt] = size(T1);
[rowc,colc] = size(CHARS);
% 
% % Length of CHARS
for j = 1:colc
    for i = 150:-1:0
        % edit t
        ch = char(CHARS{j});
        T1 =strrep(T1,strcat(ch,num2str(i)),strcat(ch,'[',num2str(i),']'));
        T3 =strrep(T3,strcat(ch,num2str(i)),strcat(ch,'[',num2str(i),']'));
    end
end
T1 = strrep(T1,'MatrixWithNoName','z');

count = 0;
for i=0:5
    for j=0:5
        T1 = strrep(T1,strcat('[',num2str(i),'][',num2str(j),']'),strcat('[',num2str(count),']'));
        count = count+1;
    end
end

fid = fopen('op','w+');

% Length of T1
for i=1:rowt
    fprintf(fid,'%s=%s\n',char(T1(i)),char(T3(i)));
end

fclose(fid);


